#ifndef SAFETY_RECHECK_H
#define SAFETY_RECHECK_H

#include <math.h>
#include "safety/safety_type.h"
#include "zuc/nml_intf/zucpos.h"
#include "motion/safezone.h"

#define JOINT_POS_THRESHOLD 10.0
#define JOINT_SPEED_THRESHOLD 20.0
#define LINEAR_SPEED_THRESHOLD 100.0
#define DIST_THRESHOLD 50.0
#define TIME_THRESHOLD 2.0
#ifdef __cplusplus
extern "C" {
#endif
typedef struct safety_double_check_inputp_t
{
    int motion_state;
    double current_jpos_fb[ZUCMOT_MAX_JOINTS];
    ZucPose tool_offset;
    ZucPose user_offset;
    PmRpy base_offset;
    double trajCycleTime;
    int dragging;
    int not_trig_abort;
    int reduce_mode;
    int tp_is_pausing;
    int safezone_confirm;
    double dec_dist;
    double dec_time;
    int now_not_run_prog;
} safety_double_check_inputp_t;

typedef struct safe_plane_violation_t
{
    int safe_plane_violation;
    int mode_when_violate;  // 0: stop_mode_violation, 1: protec_mode_violation, 2: reduce_mode_violation
    // int elbow_pos_violation;
    // int tcp_pos_violation;
} safe_plane_violation_t;

typedef struct safety_config_t
{
    double max_pos_limit[ZUCMOT_MAX_JOINTS];
    double min_pos_limit[ZUCMOT_MAX_JOINTS];
    double jvel_limit[ZUCMOT_MAX_JOINTS];

    double tcpVelLimit;
    double stoppingDist;
    double stoppingTime;
    double elbow_sphere_radius;

    double elbow_vel_limit_reduce;
    double tcp_vel_reduce;
    double stopping_time_reduce;
    double stopping_dist_reduce;

    SafeZone zone;
    zuc_set_attitude_safety tool_ori;
} safety_config_t;

typedef struct safety_info_t
{
    safety_config_t safety_config;
    // ZucPose last_carte_pos_fb;               /* 上一周期关节反馈位置计算得到的末端笛卡尔位姿, use last_jpos_fb to cal for avoiding speed jump by 坐标系变换 */
    // ZucPose carte_pos_fb;                    /* actual Cartesian position */
    // ZucPose elbow_pos_fb;
    // ZucPose last_elbow_pos_fb;     /*坐标系变换*/
    double last_jpos_fb[ZUCMOT_MAX_JOINTS]; /* 上一周期关节反馈位置 */
    double last_last_jpos_fb[ZUCMOT_MAX_JOINTS];
    int numJoints;
} safety_info_t;

void safety_func_check();

int safety_double_check(safety_double_check_inputp_t* input_status);

/*
judge jpos, return -1: jpos outof limit
*/
int safety_check_jpos_limit(double* current_jpos_fb, double* max_jpos_limit, double* min_jpos_limit);

/*
judge jvel, return -1: jvel outof limit
*/
int safety_check_jvel_limit(double* current_jvel, double* max_jvel_limit);

/*
judge tcp vel, return -1 outof limit
需要判断机器人是否处于缩减模式,根据模式取约束值
*/
int safety_check_tcp_vel_limit(ZucPose* current_cart_pos, ZucPose* last_cart_pos, ZucPose* last_last_cart_pos, safety_double_check_inputp_t* input_status);

/*
judge elbow vel, return -1 outof limit
需要判断机器人是否处于缩减模式,根据模式取约束值
*/
int safety_check_elbow_vel_limit(ZucPose* current_elbow_pos, ZucPose* last_elbow_pos, ZucPose* last_last_elbow_pos, safety_double_check_inputp_t* input_status);

/*
judge stop time, use maxacc to check, return -1 outof limit
需要判断机器人是否处于缩减模式,根据模式取约束值
TODO : 增加扭矩约束算法之后，加速度值实时变化，停止时间安全功能存在问题
*/
int safety_check_stop_time_limit(safety_double_check_inputp_t* input_status);

/*
judge stop distance, use maxacc to check, return -1 outof limit
需要判断机器人是否处于缩减模式,根据模式取约束值
TODO : 增加扭矩约束算法之后，加速度值实时变化，停止时间安全功能存在问题
*/
int safety_check_stop_dist_limit(safety_double_check_inputp_t* input_status);

/*
judge safe plane, return safe_plane_violation_t
停止、保护性停止、缩减模式,
开机启用时, 允许从非安全区域返回安全区域
肘部约束是否勾选
----------- 勾选“运行启动”时，不对直线/关节运动至该点生效 -----------
*/
safe_plane_violation_t safety_check_safe_plane(ZucPose* cur_cart_pos_to_world,
                                               ZucPose* last_cart_pos_to_world,
                                               ZucPose* cur_elbow_pos_to_world,
                                               ZucPose* last_elbow_pos_to_world,
                                               safety_double_check_inputp_t* input_status);

//return : 0 : safe 	1:dangerous
int recheck_safe_zone(SafeZone zone,
                      double r_elbow,
                      ZucPose* cur_cart_pos_to_world,
                      ZucPose* cur_elbow_pos_to_world,
                      ZucPose* last_cart_pos_to_world,
                      ZucPose* last_elbow_pos_to_world);

// 停止模式下的安全平面是否正常执行安全功能
// 停止模式：机器人处于危险区域且在远离安全区域，但控制器没有触发停止(motAbortMotion, tpAbort, zucmotDebug->enabling = 0)
int is_stop_mode_safeplane_violation();

// 保护性停止模式下的安全平面是否正常执行安全功能
// 保护性停止模式：未退出“保护性停止”时，机器人应处于减速或停止状态;
int is_protec_mode_safeplane_violation(safety_double_check_inputp_t* input_status);

//缩减模式下的安全平面是否正常执行安全功能
int is_reduce_mode_safeplane_violation();

/*
judge tool ori, return -1 
允许从非安全区域返回安全区域
检测工具姿态是否超出且远离限制时,是否motAbortMotion，tpAbort
该接口中不考虑安全距离
*/
int safety_check_tool_ori(ZucPose* cur_cart_pos_to_world, ZucPose* last_cart_pos_to_world, ZucPose* cur_tool_offsets, int not_trig_abort);

//判断是否触发tpabort、motionabort
int is_tpabort_motabort_triggered();

void trigger_abort();

void recheck_set_maxjpos(double maxjpos, int index);

void recheck_set_minjpos(double minjpos, int index);

void recheck_set_maxjvel(double maxjvel, int index);

void recheck_set_maxtcpvel(double maxtcpvel);

void recheck_set_stop_dist(double stop_dist);

void recheck_set_stop_time(double stop_time);

void recheck_set_elbow_radius(double elbow_radius);

void recheck_set_maxelbowvel_reduce(double max_elbow_vel_reduce);

void recheck_set_maxtcpvel_reduce(double maxtcpvel_reduce);

void recheck_set_stop_dist_reduce(double stop_dist_reduce);

void recheck_set_stop_time_reduce(double stop_time_reduce);

void recheck_set_safe_zone(SafeZone safe_zone);

void recheck_set_tool_ori(zuc_set_attitude_safety tool_ori);

void recheck_set_joints_num(int joints_num);
#ifdef __cplusplus
}
#endif
#endif